#pragma once
#include "rclcpp/rclcpp.hpp"
#include "libbase/common.h"
#include "base_interfaces/msg/kinematic_status_of_all.hpp"
#include "base_interfaces/msg/bt_aimer.hpp"
#include "base_interfaces/msg/kinematic_status.hpp"
#include "base_interfaces/msg/roi.hpp"
#include "base_interfaces/msg/aimer.hpp"
#include "base_interfaces/msg/scan_ctrl_info.hpp"

namespace wmj{

    /**
     * @class AimerFilter
     * @brief AimerFilter类
     * @details 用于筛选ekf模型
     * @author 吴奇优
     */
    class AimerFilter : public rclcpp::Node
    {
    public:
        /**
         * @brief 构造函数
         * @param name 节点的名称
         */
        explicit AimerFilter(std::string name);

    private:
        bool debug;                                 ///< 调试模式
        double m_distance_threshold_min;            ///< 最小距离阈值
        double m_distance_threshold_max;            ///< 最大距离阈值
        double m_distance_threshold_ignore;         ///< 筛除装甲板的最远距离阈值
        int m_dectect_num_threshold;                ///< 检测数量阈值
        std::map<int, int> mapping;                 ///< 映射表
        std::vector<int> m_priority_init;           ///< 初始优先级
        std::vector<int> priority;                  ///< 优先级
        std::vector<int> status_index_within_range; ///< 在范围内的状态索引
        int detect_num;                             ///< 检测数量
        int aimer_id;                               ///< Aimer的ID
        int m_roi_weight;                           ///< ROI宽度
        int m_roi_height;                           ///< ROI的高度
        int m_reset_num;
        bool m_should_scan;                         ///< 是否通过隧道

        rclcpp::Subscription<base_interfaces::msg::KinematicStatusOfAll>::SharedPtr status_sub; ///< status的订阅者
        rclcpp::Subscription<base_interfaces::msg::BtAimer>::SharedPtr bt_sub;            ///< 行为树bt订阅者
        rclcpp::Publisher<base_interfaces::msg::KinematicStatus>::SharedPtr status_pub_left;    ///< 左侧状态的发布者
        rclcpp::Publisher<base_interfaces::msg::KinematicStatus>::SharedPtr status_pub_right;   ///< 右侧状态的发布者
        rclcpp::Publisher<base_interfaces::msg::Roi>::SharedPtr roi_pub;                        ///< ROI的发布者
        rclcpp::Publisher<base_interfaces::msg::ScanCtrlInfo>::SharedPtr scan_pub;              ///< ScanCtrlInfo
    
    private:
        /**
         * @brief ekf模型回调函数
         * @param msg KinematicStatusOfAll消息
         */
        void status_cb(const base_interfaces::msg::KinematicStatusOfAll & msg);
        
        /**
         * @brief bt回调函数
         * @param msg BtAimer消息
         */
        void bt_cb(const base_interfaces::msg::BtAimer & msg);
        
        /**
         * @brief 重置filter
         */
        void reset();

        /**
         * @brief 设置参数
         * @param file_path 文件路径
         */
        void setParam(const std::string &file_path);
        
        /**
         * @brief 获取模型索引，并保存
         * @param status ekf模型
         * @return 是否获取成功
         */
        bool getEnableIndex(std::vector<base_interfaces::msg::KinematicStatus> status);
        
        /**
         * @brief 获取自瞄索引
         * @param status ekf模型
         * @param index_track 自瞄索引
         * @return 是否获取成功
         */
        bool getTrackIndex(std::vector<base_interfaces::msg::KinematicStatus> status, std::vector<int> &index_track);
        
        /**
         * @brief 获取目标ID
         * @param status 状态
         * @param index 索引
         * @param target_id 目标ID
         * @return 是否获取成功
         */
        bool getTargetId(std::vector<base_interfaces::msg::KinematicStatus> status, std::vector<int> index, int &target_id);
        
        /**
         * @brief 查找最高优先级的ID
         * @param id ID
         * @return 最高优先级的ID
         */
        int findHighestPriorityId(std::vector<int> id);
        
        /**
         * @brief 更新优先级
         * @param id ID
         * @return 是否更新成功
         */
        bool updatePriority(int id);
        
        /**
         * @brief 发布ROI
         * @param roi ROI
         * @param frame_id 相机坐标系
         */
        void publishRoi(cv::Rect2f roi, std::string frame_id);
        
        /**
         * @brief 发布ScanMode
         * @param scan_mode scan mode
         */
        void publishScanMode(int scan_mode);
    };
} //namespace wmj